/**************************************************************************************************
* Title: SmurfsPhysicsManager.cpp
* Authors: Gael Huber, Erin Mooney
* Description: Manages updating of physics
**************************************************************************************************/
#include "SmurfsPhysicsManager.h"
#include "SmurfsRenderManager.h"
#include "SimulationManager.h"

// Singleton instance
template<> PhysicsManager* Singleton<PhysicsManager>::singleton = 0;

/**************************************************************************************************
* Constructor
**************************************************************************************************/
PhysicsManager::PhysicsManager(void) {
	// Do nothing
}

/**************************************************************************************************
* Destructor
**************************************************************************************************/
PhysicsManager::~PhysicsManager(void) {
	// Do nothing
}

/**************************************************************************************************
* Initialize physics
**************************************************************************************************/
void PhysicsManager::startUp(void) {
	this->foundCollision = false;
	timer = new Timer();
	timer->setupTimer();
	timer->setStartTime();
}

/**************************************************************************************************
* Shutdown
**************************************************************************************************/
void PhysicsManager::shutDown(void) {

}

/**************************************************************************************************
* Get the singleton instance
**************************************************************************************************/
PhysicsManager& PhysicsManager::getSingleton(void) { 
	assert(singleton);
	return *singleton;
}

/**************************************************************************************************
* Get the singleton pointer
**************************************************************************************************/
PhysicsManager* PhysicsManager::getSingletonPtr(void) {
	return singleton;
}

/**************************************************************************************************
* Update the scene
**************************************************************************************************/
void PhysicsManager::update(void) {
	// Get elapsed time since last frame
	dt = timer->getDeltaTime();

	// Define start and end of vector
	std::vector<RigidBody*>::iterator begin = bodyList.begin();
	std::vector<RigidBody*>::iterator end = bodyList.end();

	// Iterate through the list and update each object
	for(std::vector<RigidBody*>::iterator itr = begin; itr != end; ++itr) {
		// Respond to collisions
		if(SimulationManager::getSingletonPtr()->getPlayer()->getBody() == *itr)
			collideAndSlide((Sphere*)*itr);

		else {
			// If it's not the player, do moment of inertia calculations
			(*itr)->updateForces(dt);

			// Try to collide with ground
			collideAndSlide((Sphere*)*itr);
		}

		// Either way, check collisions with other objects
		for(std::vector<RigidBody*>::iterator itr2 = itr + 1; itr2 != end; ++itr2) {
			Vector3 p = Vector3();
			if(((*itr)->center - (*itr2)->center).squaredLength() <= ((Sphere*)(*itr))->radius + (*itr2)->halfWidths.squaredLength() * 2.0f) {//((Sphere*)(*itr2))->radius) {
			//if(testSphereOBB((Sphere*)*itr, *itr2, p)) {
				// If collision, impart momentum
/*				(*itr)->velocity = (*itr)->velocity * ((*itr)->mass - (*itr2)->mass)/((*itr)->mass + (*itr2)->mass) + 
					(*itr2)->velocity * (2.0f * (*itr2)->mass)/((*itr)->mass + (*itr2)->mass);
				(*itr2)->velocity = (*itr2)->velocity * ((*itr2)->mass - (*itr)->mass)/((*itr)->mass + (*itr2)->mass) + 
					(*itr)->velocity * (2.0f * (*itr)->mass)/((*itr)->mass + (*itr2)->mass);
*/				
				// Calculate total momentum of both objects
				Vector3 l1 = (*itr)->velocity * (*itr)->mass;
				Vector3 l2 = (*itr2)->velocity * (*itr2)->mass;
				Vector3 l = l1 + l2;

				// Set velocities
				(*itr)->velocity = l / (*itr)->mass;
				(*itr2)->velocity = l / (*itr2)->mass;
			}
		}
	}
}

/**************************************************************************************************
* Add an object to the scene
**************************************************************************************************/
void PhysicsManager::addToWorld(RigidBody* obj) {
	bodyList.push_back(obj);
}

/**************************************************************************************************
* Remove an object from the scene
**************************************************************************************************/
void PhysicsManager::removeFromWorld(RigidBody* obj) {
	std::vector<RigidBody*>::iterator begin = bodyList.begin();
	std::vector<RigidBody*>::iterator end = bodyList.end();

	// Iterate through the list and update each object
	for(std::vector<RigidBody*>::iterator itr = begin; itr != end; ++itr) {
		if(obj == *itr) {
			bodyList.erase(itr);
			return;
		}
	}
}

/**************************************************************************************************
* Must be done separately from rest of world as the timer run internal to physics must have a very 
* short interval.
**************************************************************************************************/
void PhysicsManager::initializeTimer(void) {
	timer->getDeltaTime();
}

/**************************************************************************************************
* Check for a collision between a sphere and a triangle
**************************************************************************************************/
bool PhysicsManager::triangleSphereCollision(Sphere* s, SmurfsPolygon* t) {
	// We want to ensure that everything is in relation to the sphere as a unit sphere, so divide the
	// position of the triangle by the position of the sphere
	Vector3 triangle = t->center / s->radius;
//	triangle /= s->radius;

	// Velocity
	Vector3 velocity = s->velocity / s->radius;
	
	// Base point in ellipsoid space
	Vector3 basePoint = s->center / s->radius;
	
	// Triangle points also brought into ellipsoidal space
	Vector3 trianglePoints[3];
	trianglePoints[0] = t->getPoints()[0] / s->radius;
	trianglePoints[1] = t->getPoints()[1] / s->radius;
	trianglePoints[2] = t->getPoints()[2] / s->radius;

	// The first step is to see whether the swept sphere will even collide with the plane, so let's 
	// construct a plan from the triangle
	SmurfsPlane p = SmurfsPlane(trianglePoints[0], trianglePoints[1], trianglePoints[2]);

	// Next calculate the signed distance 
	float signedDistance = p.signedDistanceTo(basePoint);
	
	// Calculate the point at which the sphere will first hit the triangle if at all
	bool embeddedInPlane = false;
	float normalDotVelocity = p.getNormal()->dot(velocity);
	float t0 = 0.0f;
	float t1 = 0.0f;

	// If the dot product between the plane's normal and the sphere's velocity is 0, then the sphere
	// is traveling parallel to the plane. This leaves us with two possibilities. First, the sphere 
	// is embedded in the plane at which point t0 = 0 and t1 = 1 since there is always an 
	// intersection. Second, the absolute distance from the plane is greater than 1 and the sphere is
	// never colliding, so we can return.
	if(normalDotVelocity == 0) {
		if(abs(signedDistance) >= 1.0f)
			return false;
		else {
			t0 = 0.0f;
			t1 = 1.0f;
			embeddedInPlane = true;
		}
	} else {
		t0 = (1.0f - signedDistance) / normalDotVelocity;
		t1 = (-1.0f - signedDistance) / normalDotVelocity;

		// Swap to t0 < t1
		if(t0 > t1) {
			float temp = t1;
			t1 = t0;
			t0 = temp;
		}

		// Check that at least one result is within range:
		if(t0 > 1.0f || t1 < 0.0f) {
			// Both t values are outside values [0, 1], so no collisions are possible
			return false;
		}

		// Clamp to [0, 1]
		if(t0 < 0.0f) t0 = 0.0f;
		if(t1 < 0.0f) t1 = 0.0f;
		if(t0 > 1.0f) t0 = 1.0f;
		if(t1 > 1.0f) t1 = 1.0f;

		// There are now three cases for a potential collision:
		// 1. The sphere can collide with the inside of the triangle.
		// 2. The sphere can collide against one of the three vertices of the triangle.
		// 3. The sphere can collide against one of the three edges of the triangle.
		Vector3 collisionPoint;
		bool foundCollision = false;
		float time = 1.0f;

		// Let us first look at the first case, since it can be calculated quickly, and therefore we can 
		// hopefully skip more expensive tests.
		if(!embeddedInPlane) {
			Vector3 planeIntersectionPoint = basePoint - *p.getNormal() + velocity * t0;

			// Check whether the intersection point lies within the triangle
			if(checkPointInTriangle(planeIntersectionPoint, trianglePoints[0], trianglePoints[1], trianglePoints[2])) {
				foundCollision = true;
				time = t0;
				collisionPoint = planeIntersectionPoint;
			}
		} else {
			// We are embedded in the plane, use a quick hack to increase the height by a very small margin
		//	s->center.y += 0.1f;
		}

		// If we haven't found a collision already we'll have to sweep sphere against points and edges of
		// the triangle. Note: A collision inside the triangle (the check above) will always happen 
		// before a vertex or edge collision! This is why we can skip the swept test if the above gives a
		// collision!
		if(foundCollision == false) {
			// Some commonly used variables
			Vector3 base = s->center;
			float velocitySquaredLength = velocity.squaredLength();
			float a,b,c;	// Parameters for quadratic
			float newT;

			// For each vertex or edge a quadratic equation has to be solved. We parametrize this equation as 
			// a*t^2 + b*t + c = 0 and below we calculate hte parameters a, b, and c for each test.
			
			// Check against points:
			a = velocitySquaredLength;

			// P1
			b = 2.0f * (velocity.dot(base - trianglePoints[0]));
			c = (trianglePoints[0] - base).squaredLength() - 1.0f;

			if(getLowestRoot(a, b, c, time, &newT)) {
				time = newT;
				foundCollision = true;
				collisionPoint = trianglePoints[0];
			}

			// P2
			b = 2.0f * (velocity.dot(base - trianglePoints[1]));
			c = (trianglePoints[1] - base).squaredLength() - 1.0f;

			if(getLowestRoot(a, b, c, time, &newT)) {
				time = newT;
				foundCollision = true;
				collisionPoint = trianglePoints[1];
			}

			// P3
			b = 2.0f * (velocity.dot(base - trianglePoints[2]));
			c = (trianglePoints[2] - base).squaredLength() - 1.0f;

			if(getLowestRoot(a, b, c, time, &newT)) {
				time = newT;
				foundCollision = true;
				collisionPoint = trianglePoints[2];
			}

			// Check against edges:
			// P1 -> P2
			Vector3 edge = trianglePoints[1] - trianglePoints[0];
			Vector3 baseToVertex = trianglePoints[0] - base;
			float edgeSquaredLength = edge.squaredLength();
			float edgeDotVelocity = edge.dot(velocity);
			float edgeDotBaseToVertex = edge.dot(baseToVertex);

			// Calculate parameters for equation
			a = edgeSquaredLength * -velocitySquaredLength + edgeDotVelocity * edgeDotVelocity;
			b = edgeSquaredLength * (2.0f * velocity.dot(baseToVertex)) - 2.0f * edgeDotVelocity * edgeDotBaseToVertex;
			c = edgeSquaredLength * (1.0f - baseToVertex.squaredLength()) + edgeDotBaseToVertex * edgeDotBaseToVertex;

			// Does the swept sphere collide against infinite edge?
			if(getLowestRoot(a, b, c, time, &newT)) {
				// Check if intersection is within line segment
				float f = (edgeDotVelocity * newT - edgeDotBaseToVertex) / edgeSquaredLength;

				if(f >= 0.0f && f <= 1.0f) {
					// Intersection took place within segment
					time = newT;
					foundCollision = true;
					collisionPoint = trianglePoints[0] + edge * f;
				}
			}

			// P2 -> P3
			edge = trianglePoints[2] - trianglePoints[1];
			baseToVertex = trianglePoints[1] - base;
			edgeSquaredLength = edge.squaredLength();
			edgeDotVelocity = edge.dot(velocity);
			edgeDotBaseToVertex = edge.dot(baseToVertex);

			a = edgeSquaredLength * -velocitySquaredLength + edgeDotVelocity * edgeDotVelocity;
			b = edgeSquaredLength * (2.0f * velocity.dot(baseToVertex)) - 2.0f * edgeDotVelocity * edgeDotBaseToVertex;
			c = edgeSquaredLength * (1.0f - baseToVertex.squaredLength()) + edgeDotBaseToVertex * edgeDotBaseToVertex;

			if(getLowestRoot(a, b, c, time, &newT)) {
				// Check if intersection is within line segment
				float f = (edgeDotVelocity * newT - edgeDotBaseToVertex) / edgeSquaredLength;

				if(f >= 0.0f && f <= 1.0f) {
					// Intersection took place within segment
					time = newT;
					foundCollision = true;
					collisionPoint = trianglePoints[1] + edge * f;
				}
			}

			// P3 -> P1
			edge = trianglePoints[0] - trianglePoints[2];
			baseToVertex = trianglePoints[2] - base;
			edgeSquaredLength = edge.squaredLength();
			edgeDotVelocity = edge.dot(velocity);
			edgeDotBaseToVertex = edge.dot(baseToVertex);

			a = edgeSquaredLength * -velocitySquaredLength + edgeDotVelocity * edgeDotVelocity;
			b = edgeSquaredLength * (2.0f * velocity.dot(baseToVertex)) - 2.0f * edgeDotVelocity * edgeDotBaseToVertex;
			c = edgeSquaredLength * (1.0f - baseToVertex.squaredLength()) + edgeDotBaseToVertex * edgeDotBaseToVertex;

			if(getLowestRoot(a, b, c, time, &newT)) {
				// Check if intersection is within line segment
				float f = (edgeDotVelocity * newT - edgeDotBaseToVertex) / edgeSquaredLength;

				if(f >= 0.0f && f <= 1.0f) {
					// Intersection took place within segment
					time = newT;
					foundCollision = true;
					collisionPoint = trianglePoints[2] + edge * f;
				}
			}
		}

		// Set result
		if(foundCollision == true) {
			// Distance to collision: t is time of collision
			float distToCollision = velocity.magnitude() * time;

			// Does this triangle quality for the closest hit? It does if it's the first hit or the closest.
			if(this->foundCollision == false || distToCollision < this->nearestDistance) {
				this->nearestDistance = distToCollision;
				this->intersectionPoint = collisionPoint;
				this->foundCollision = true;
				terrainType = t->getType();
				return true;
			}
		}
	}	// If not backface

	return false;
}

/**************************************************************************************************
* Have a body respond to a collision.
**************************************************************************************************/
void PhysicsManager::collideAndSlide(Sphere* body) {
	// Reset the terrain type
	terrainType = NONE;

	// Cap velocity
	if(body->velocity.magnitude() > 100.0f) {
		body->velocity.normalize();
		body->velocity *= 100.0f;
	}

	// Calculate position and velocity in e-space
	Vector3 eSpace = body->center / body->radius;
	Vector3 eVelocity = body->velocity / body->radius;

	// Iterate until we have our final position
	collisionRecursionDepth = 0;
	Vector3 finalPosition = collideWithWorld(body, eSpace, eVelocity * dt);

	// Add gravity pull:
	// Set the new R3 position
	body->center = finalPosition * body->radius;
	body->velocity += Vector3(0, GRAVITY, 0);

	eVelocity = body->velocity / body->radius;
	collisionRecursionDepth = 0;

	finalPosition = collideWithWorld(body, finalPosition, eVelocity * dt);

	// Convert final result back to R3
	finalPosition = finalPosition * body->radius;

	// Move the entity
	body->center = finalPosition;

	// Decrement velocity by the appropriate amount
	applyFriction(body);

	if(body == (Sphere*)SimulationManager::getSingletonPtr()->getPlayer()->getBody()) {
		glm::vec3 v = glm::vec3(body->center.x, body->center.y + 0.03, body->center.z);
		v *= -1.0f;
		RenderManager::getSingletonPtr()->getCamera()->setPosition(v);
	}
}

/**************************************************************************************************
* Collide with the world!
**************************************************************************************************/
Vector3 PhysicsManager::collideWithWorld(Sphere* body, Vector3 position, Vector3 velocity) {
	// All hard-coded distances in this function are scaled to fit the setting above.
	float unitScale = 1.0f;
	float veryCloseDistance = 0.005f * unitScale;

	// Do we need to worry?
	if(collisionRecursionDepth > 5)
		return position;

	// Ok, we need to worry
	this->foundCollision = false;
	this->nearestDistance = FLT_MAX;
	this->eVelocity = velocity;
	this->eNormalizedVelocity = velocity;
	this->eNormalizedVelocity.normalize();

	// Find collisions
	testCollisions(body);

	// If no collision, we just move along the velocity
	if(this->foundCollision == false)
		return (position + velocity);

	// A collision has occurred!!!!

	// Th original destination position
	Vector3 destinationPoint = position + velocity;
	Vector3 newBasePoint = position;

	// Only update if we are not already very close and if so we only move very close to the 
	// intersection, not to the exact spot
	if(nearestDistance >= veryCloseDistance) {
		Vector3 v = velocity;
		v = v.getNormalized() * (nearestDistance * veryCloseDistance);
		newBasePoint = body->center / body->radius + v;

		// Adjust polygon intersection point (so sliding plane will be unaffeted by the face that we move
		// slightly less than collision tells us)		
		v.normalize();
		this->intersectionPoint -= v * veryCloseDistance;
	}

	// Determine the sliding plane
	Vector3 slidePlaneOrigin = intersectionPoint;
	Vector3 slidePlaneNormal = newBasePoint - intersectionPoint;
	slidePlaneNormal.normalize();
	SmurfsPlane slidingPlane(slidePlaneOrigin, slidePlaneNormal);

	// Calculate the new destination point
	Vector3 newDestinationPoint = destinationPoint - slidePlaneNormal * slidingPlane.signedDistanceTo(destinationPoint);
	
	// Generate the new slide vector, which will become our new velocity vector for the next iteration
	Vector3 newVelocityVector = newDestinationPoint - intersectionPoint;
	body->velocity = newVelocityVector * body->radius;

	// Recuse:
	// Don't recurse if the new velocity is very small
	if(newVelocityVector.magnitude() < veryCloseDistance) {
		return newBasePoint;
	}

	collisionRecursionDepth++;
	return collideWithWorld(body, newBasePoint, newVelocityVector);
}

/**************************************************************************************************
* Check a single object for collisions against the world
**************************************************************************************************/
void PhysicsManager::testCollisions(Sphere* body) {
	// Test against the terrain
	testCollisionsTerrain(body, tree->root);

	// Delete the ancestor stack
	//delete[] ancestorStack;
}

/**************************************************************************************************
* Test for collisions against the terrain
**************************************************************************************************/
void PhysicsManager::testCollisionsTerrain(Sphere* body, OctreeNode* tree) {
	// Depth == 0 is invariant for other calls
	static int depth = 0;

	float treeDistance = (tree->center - body->center).magnitude();
	float halfSize = tree->halfWidth.magnitude();
	if(treeDistance <= halfSize) {
		// Check collision between all objects on this level
		ancestorStack[depth++] = tree;
	
		std::vector<SmurfsPolygon*>::iterator begin = tree->polygons.begin();
		std::vector<SmurfsPolygon*>::iterator end = tree->polygons.end();
		for(std::vector<SmurfsPolygon*>::iterator itr = begin; itr != end; ++itr) {
			// First parse by distance. If the distance is greater than the length of the velocity vector 
			// plus both radii no checks are necessary, this will cull save a lot of unnecessary checks.
			float distance = (body->center - (*itr)->center).magnitude();
			if(distance <= body->radius + (*itr)->getRadius() + body->velocity.magnitude() * dt) {
				// Test collision and update collisoin packet information
				triangleSphereCollision(body, *itr);
			}
		}

		// Visit the appropriate child
		int index = 0;
		for(int i = 0; i < 3; i++) {
			float delta = body->center[i] - tree->center[i];
			if(abs(delta) <= body->radius) {
				index = 8;
				break;
			} 
		
			if(delta > 0.0f) {
				index |= (1 << i);	// ZXY
			}
		}
	
		// If straddling, iterate through all 8 quadrants
		if(index == 8) {
			for(int i = 0; i < 8; i++) {
				if(tree->child[i])
					testCollisionsTerrain(body, tree->child[i]);
			}
		}

		// Otherwise check the one that is important
		else if(tree->child[index] ) {
			testCollisionsTerrain(body, tree->child[index]);
		}

		// Remove current node from ancestor stack before returning
		depth--;
	} else {
		// Assuming that the depth is zero (i.e. the main tree, we know that we must now generate a new 
		// terrain and delete the current one as we have "abandoned" our world.
		if(depth == 0)
			SimulationManager::getSingletonPtr()->regenPlayer();

	}
}

/**************************************************************************************************
* Returns true if sphere s intersects OBB b, false otherwise. The point p on the OBB closest to the
* sphere center is also returned.
**************************************************************************************************/
bool PhysicsManager::testSphereOBB(Sphere* s, RigidBody* b, Vector3 &p) {
	// Find point p on the OBB cloest to sphere center
	b->closestPtPointOBB(s->center, p);

	// Sphere and OBB intersect if the (squared) distance from sphere center to point p is less than
	// the (squared) sphere radius.
	Vector3 v = p - s->center;
	return v.dot(v) <= s->radius * s->radius;
}

/**************************************************************************************************
* Test for collision between two OBBs
**************************************************************************************************/
bool PhysicsManager::testObbObb(RigidBody &a, RigidBody &b) {
	float ra, rb;
	Matrix R = Matrix(3, 3);
	Matrix absR = Matrix(3, 3);

	// Create unit vectors representing each axis of either rigid body
	Vector3 au[3] = {Vector3(1.0f, 0, 0), Vector3(0, 1.0f, 0), Vector3(0, 0, 1.0f)};
	Vector3 bu[3] = {Vector3(1.0f, 0, 0), Vector3(0, 1.0f, 0), Vector3(0, 0, 1.0f)};
	au[0].rotate(a.axes); au[1].rotate(a.axes); au[2].rotate(a.axes);
	bu[0].rotate(b.axes); bu[1].rotate(b.axes); bu[2].rotate(b.axes); 

	// Computer rotation matrix expressing b in a's coordinate frame
	for(int i = 0; i < 3; i++) {
		for(int j = 0; j < 3; j++) {
			R.matrix[i][j] = au[i].dot(bu[i]);
		}
	}

	// Compute translation vector t
	Vector3 t = b.center - a.center;

	// Bring translation vector into a's coordinate frame
	t = Vector3(t.dot(au[0]), t.dot(au[1]), t.dot(au[2]));

	// Compute common subexpressions. Add in an epsilon term to counteract arithmetic errors when two
	// edges are parallel and their cross product is (near) null.
	for(int i = 0; i < 3; i++) {
		for(int j = 0; j < 3; j++) {
			absR.matrix[i][j] = abs(R.matrix[i][j] + FLT_EPSILON);
		}
	}

	// Test axes L = A0, L = A1, L = A2
	for(int i = 0; i < 3; i++) {
		ra = a.halfWidths[i];
		rb = (b.halfWidths[0] * absR.matrix[i][0]) + (b.halfWidths[1] * absR.matrix[i][1]) + (b.halfWidths[2] * absR.matrix[i][2]);
		if(abs(t[i]) > ra + rb)
			return 0;
	}

	// Test axis L = A0 x B0
	ra = (a.halfWidths[1] * absR.matrix[2][0]) + (a.halfWidths[2] * absR.matrix[1][0]);
	rb = (b.halfWidths[1] * absR.matrix[0][2]) + (a.halfWidths[2] * absR.matrix[0][1]);
	if(abs((t[2] * R.matrix[1][0]) - (t[1] * R.matrix[2][0])) > ra + rb)
		return false;

	// Test axis L = A0 x B1
	ra = (a.halfWidths[1] * absR.matrix[2][1]) + (a.halfWidths[2] * absR.matrix[1][1]);
	rb = (b.halfWidths[0] * absR.matrix[0][2]) + (b.halfWidths[2] * absR.matrix[0][0]);
	if(abs((t[2] * R.matrix[1][1]) - (t[1] * R.matrix[2][1])) > ra + rb)
		return false;

	// Test axis L = A0 x B2
	ra = (a.halfWidths[1] * absR.matrix[2][2]) + (a.halfWidths[2] * absR.matrix[1][2]);
	rb = (b.halfWidths[0] * absR.matrix[0][1]) + (b.halfWidths[1] * absR.matrix[0][0]);
	if(abs((t[2] * R.matrix[1][2]) - (t[1] * R.matrix[2][2])) > ra + rb)
		return false;

	// Test axis L = A1 x B0
	ra = (a.halfWidths[0] * absR.matrix[2][0]) + (a.halfWidths[2] * absR.matrix[0][0]);
	rb = (b.halfWidths[1] * absR.matrix[1][2]) + (b.halfWidths[2] * absR.matrix[1][1]);
	if(abs((t[0] * R.matrix[2][0]) - (t[2] * R.matrix[0][0])) > ra + rb)
		return false;

	// Test axis L = A1 x B1
	ra = (a.halfWidths[0] * absR.matrix[2][1]) + (a.halfWidths[2] * absR.matrix[0][1]);
	rb = (b.halfWidths[0] * absR.matrix[1][2]) + (b.halfWidths[2] * absR.matrix[1][0]);
	if(abs((t[0] * R.matrix[2][1]) - (t[2] * R.matrix[0][1])) > ra + rb)
		return false;

	// Test axis L = A1 x B2
	ra = (a.halfWidths[0] * absR.matrix[2][2]) + (a.halfWidths[2] * absR.matrix[0][2]);
	rb = (b.halfWidths[0] * absR.matrix[1][1]) + (b.halfWidths[1] * absR.matrix[1][0]);
	if(abs((t[0] * R.matrix[2][2]) - (t[2] * R.matrix[0][2])) > ra + rb)
		return false;

	// Test axis L = A2 x B0
	ra = (a.halfWidths[0] * absR.matrix[1][0]) + (a.halfWidths[1] * absR.matrix[0][0]);
	rb = (b.halfWidths[1] * absR.matrix[2][2]) + (b.halfWidths[2] * absR.matrix[2][1]);
	if(abs((t[1] * R.matrix[0][0]) - (t[0] * R.matrix[1][0])) > ra + rb)
		return false;

	// Test axis L = A2 x B1
	ra = (a.halfWidths[0] * absR.matrix[1][1]) + (a.halfWidths[1] * absR.matrix[0][1]);
	rb = (b.halfWidths[0] * absR.matrix[2][2]) + (b.halfWidths[2] * absR.matrix[2][0]);
	if(abs((t[1] * R.matrix[0][1]) - (t[0] * R.matrix[1][1])) > ra + rb)
		return false;
	
	// Test axis L = A2 x B2
	ra = (a.halfWidths[0] * absR.matrix[1][2]) + (a.halfWidths[1] * absR.matrix[0][2]);
	rb = (b.halfWidths[0] * absR.matrix[2][1]) + (b.halfWidths[1] * absR.matrix[2][0]);
	if(abs((t[1] * R.matrix[0][2]) - (t[0] * R.matrix[1][2])) > ra + rb)
		return false;

	// Since no separating axis is found, the OBBs must be intersecting
	return true;
}

/**************************************************************************************************
* Check whether a point lies inside the given triangle
**************************************************************************************************/
bool PhysicsManager::checkPointInTriangle(Vector3 point, Vector3 pa, Vector3 pb, Vector3 pc) {
	Vector3 e10 = pb - pa;
	Vector3 e20 = pc - pa;

	float a = e10.dot(e10);
	float b = e10.dot(e20);
	float c = e20.dot(e20);
	float ac_bb = (a*c) - (b*b);
	Vector3 vp(point.x - pa.x, point.y - pa.y, point.z - pa.z);

	float d = vp.dot(e10);
	float e = vp.dot(e20);
	float x = (d*c) - (e*b);
	float y = (e*a) - (d*b);
	float z = x + y - ac_bb;

	bool abcd = (( ((unsigned int&)z)& ~(((unsigned int&)x)|((unsigned int&)y)) ) & 0x80000000)?true:false;

	return (( ((unsigned int&)z)& ~(((unsigned int&)x)|((unsigned int&)y)) ) & 0x80000000)?true:false;
}

/**************************************************************************************************
* Solve a quadratic equation and return the lowest root below a certain threshhold (maxR).
**************************************************************************************************/
bool PhysicsManager::getLowestRoot(float a, float b, float c, float maxR, float* root) {
	// Check if a solution exists by finding the determinant and making sure it exceeds or is greater
	// than 0 (cannot take root of a negative)
	float determinant = b*b - 4.0f*a*c;

	// If the determinant is negative, there is no solution
	if(determinant < 0.0f)
		return false;

	// Calculate the two roots: (if determinant == 0 then r1 = r2, but let's disregard that slight
	// optimization for now)
	float sqrtD = sqrt(determinant);
	float r1 = (-b - sqrtD) / (2*a);
	float r2 = (-b + sqrtD) / (2*a);

	// Sort so r1 <= r2
	if(r1 > r2) {
		float temp = r2;
		r2 = r1;
		r1 = temp;
	}

	// Get lowest root
	if(r1 > 0 && r1 < maxR) {
		*root = r1;
		return true;
	}

	// It is possible that we want r2 -- this can happen if r1 < 0
	if(r2 > 0 && r2 < maxR) {
		*root = r2;
		return true;
	}

	// No valid solutions
	return false;
}

/**************************************************************************************************
* Builds a tree to hold the terrain for collision detection
**************************************************************************************************/
void PhysicsManager::buildTerrainTree(SmurfsPolygon*** polys, int width, int height, 
	Vector3 treeCenter, Vector3 treeSize) {
	float maxX = 0.0f;
	float minX = 0.0f;
	float maxY = 0.0f;
	float minY = 0.0f;
	float maxZ = 0.0f;
	float minZ = 0.0f;
	// BSP TREE //
	// put the terrain polys into a vector
	std::vector<SmurfsPolygon*> polygons;
	for(int i = 0; i < width; i++) {
		for(int j = 0; j < height; j++) {
			polygons.push_back(polys[j][i]);
			if(polys[j][i]->center.x > maxX)
				maxX = polys[j][i]->center.x;
			if(polys[j][i]->center.x < minX)
				minX = polys[j][i]->center.x;
			if(polys[j][i]->center.y > maxY)
				maxY = polys[j][i]->center.y;
			if(polys[j][i]->center.y < minY)
				minY = polys[j][i]->center.y;
			if(polys[j][i]->center.z > maxZ)
				maxZ = polys[j][i]->center.z;
			if(polys[j][i]->center.z < minZ)
				minZ = polys[j][i]->center.z;
		}
	}

	// Initialize the tree
	tree = new Octree();

	// Build the skeleton for the tree
	tree->root = tree->buildTree(treeCenter, treeSize / 2.0f, 7);

	// Iterate through the polygons and add each one to the tree
	std::vector<SmurfsPolygon*>::iterator begin = polygons.begin();
	std::vector<SmurfsPolygon*>::iterator end = polygons.end();
	for(std::vector<SmurfsPolygon*>::iterator itr = begin; itr != end; ++itr) {
		// Insert the tree
		tree->insertObject(tree->root, *itr, 0);
	}

	// build the tree
	//treeRoot = BSPNode::buildBSPTree(polygons, 0);

	
	// Define the ancestor stack for the terrain tree
	ancestorStack = new OctreeNode*[tree->depth];
}

void PhysicsManager::destroyTerrainTree(void) {
	tree = NULL;
	ancestorStack = NULL;
}

/**************************************************************************************************
* Apply friction to a sphere.
**************************************************************************************************/
void PhysicsManager::applyFriction(Sphere* body) {
	// Iterate through different types, different effects depending
	switch(terrainType) {
		case SNOW:
			body->velocity *= 0.9f;
			break;
		case ROCK:
			body->velocity *= 0.997f;
			break;
		case ICE:
			body->velocity *= 1.05f;
			break;
		case GRASS:
			body->velocity *= 0.998f;
			break;
		case SAND:
			body->velocity *= 0.94f;
			break;
		case WATER:
			body->velocity *= 0.88f;
			break;
		case NONE:
			// No effect
			break;
	}
}